# coding=utf-8
import time

import RPi.GPIO as GPIO
from raspberryPi.Constant import *
from raspberryPi.MotroModule.Motor import Motor
from raspberryPi.AvoidModule.Avoid import UltrasonicAvoid, InfraredAvoid
from raspberryPi.SerialModule.Serial import Serial
from raspberryPi.SteeringModule.Sterring import Steering
from raspberryPi.TrackModule.Track import Track

# 设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)

GPIO.setwarnings(False)

motor = Motor(ENA, AIN1, AIN2, ENB, BIN1, BIN2)  # 电机
ultrasonicAvoid = UltrasonicAvoid(EchoPin, TrigPin)     # 超声波避障
infraredAvoid = InfraredAvoid(AvoidSensorLeft, AvoidSensorRight)    # 红外避障
track = Track()     # 循迹
steering = Steering(ServoPin)   # 舵机控制
serial = Serial(ultrasonicAvoid, infraredAvoid, track, steering)    # 串口处理数据


def action():
    """
    行动
    :return: None
    """
    if serial.g_CarState == enSTOP:
        motor.brake()
    elif serial.g_CarState == enRUN:
        motor.forward()
    elif serial.g_CarState == enLEFT:
        motor.left()
    elif serial.g_CarState == enRIGHT:
        motor.right()
    elif serial.g_CarState == enBACK:
        motor.back()
    elif serial.g_CarState == enTLEFT:
        motor.spin_left()
    elif serial.g_CarState == enTRIGHT:
        motor.spin_right()
    else:
        motor.brake()


def main():
    timeCount = 2000
    count = 100
    try:
        while True:
            serial.serialEvent()
            if serial.NewLineReceived == 1:
                # print "serialdata:%s" % serial.InputString
                serial.serial_data_parse()
                serial.NewLineReceived = 0
                action()

            timeCount -= 1
            if timeCount == 0:
                count -= 1
                timeCount = 2000
                if count == 0:
                    serial.serial_data_postback()
                    timeCount = 2000
                    count = 100
    except KeyboardInterrupt:
        pass
    serial.ser.close()
    motor.PWM_ENA.stop()
    motor.PWM_ENB.stop()
    GPIO.cleanup()

    
main()
